/*
 * FormationSquare.java
 *
 * Created on 18 de noviembre de 2007, 07:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors.Formations;


import Utils.Vector2d;
/**
 *
 * @author alejo
 */
public class FormationSquare extends Formation {
    
    
    /** Creates a new instance of FormationSquare */
    public FormationSquare(int numRobots) {
        super(numRobots);
        setWidth(2); //width of form 
        this.setName("SQUARE");
    }
    
    public Vector2d getRobotPosition(int robotId){
        if(robotId>=0 && robotId<=this.getNumRobots()){
            
            //get angle of location in Formation
            float theta = (float)getRobotLocationDir(robotId);
            
            return getVectorInForm(theta, robotId);
            
        }
            
        return null;
    
    }
    
    
    public Vector2d getVectorInForm(float theta,int robotId){
        
        float x = 0;
        float y = 0;
        
        /*
        switch(robotId){
            
            case 0:
                x = (float)(Math.sqrt(2)/2.0f);
                y = (float)(Math.sqrt(2)/2.0f);
            break;
            case 1:
                x = -(float)(Math.sqrt(2)/2.0f);
                y = (float)(Math.sqrt(2)/2.0f);
            break;
            case 2:
                x = -(float)(Math.sqrt(2)/2.0f);
                y = -(float)(Math.sqrt(2)/2.0f);
            break;
            case 3:
                x = (float)(Math.sqrt(2)/2.0f);
                y = -(float)(Math.sqrt(2)/2.0f);
            break;
            
        }
         * */
        //ESTO ESTA BUENO NO BORRAR!!!!!!!!!!
        if(theta >= -Math.PI/4 && theta <= Math.PI/4){
            x = 1;
            y = (float)Math.tan(theta);
        
        }
        else if(theta > Math.PI / 4 && theta <= (Math.PI - Math.PI / 4) ){
        
            x = (float) (1 / Math.tan(theta));
            y = 1;
        }
        else if(theta > (Math.PI - Math.PI / 4) && theta <= (Math.PI + Math.PI / 4) ){
        
            x = -1;
            y = (float)-Math.tan(theta);
        }
        else{
        
            x = (float) ( -1 / Math.tan(theta));
            y = -1;
        }
        
        if(robotId>=this.getNumRobots())
            return null;
        else
            return new Vector2d(x,y,Vector2d.MODE_XY);
        
    
    }
    

   
    public void calculateScaleFactor()
    {
        scaleFactor = (float)Math.sqrt((double)(width*width/2));
    }
    
    
}
